#include "Motion.h"
#include "sys.h"

void Motion_CTRL(short int Control, u16 FeedBack, u8 PAR_DirUpAccel, u8 PAR_DirUpDecel, u8 PAR_DirDownAccel, u8 PAR_DirDownDecel,
				 u16 PAR_DirUpMaxCurrent, u16 PAR_DirUpMinCurrent, u16 PAR_DirDownMinCurrent, u16 PAR_DirDownMaxCurrent, u8 Enable, u16 *PWM_Up, u16 *PWM_Down, u8 *p_u8, int *p_int)
{
	u8 DI_LimitSwitchUp = 0;
	u8 DI_LimitSwitchDown = 0;
	u8 PAR_CoilResistance = 24;
	u16 PAR_EndattenuationValue = 100, fll = 17857; //(*14285*);(*ClockFreq/8/140*)
	u8 PAR_SupplyVoltage = 24;
	u16 PAR_FaultOnDelay = 1;
	u8 PAR_PercentDifferenceInImpedance = 0;
	u8 CtrlAct, FaultActiveDir1, FaultActiveDir2;
	u16 Af, Ar, Df, Dr;
	int Kf, Zf, Kr, Zr, Rr;
	int JoyFull = 16384, I_MAX = 1000, I_Full = 63439, I_Nom = 72207, Full = 17857;

	static char nnn, aaa;
	//
	u8 DO_SupplyVoltage, DO_DirUpValveImpedanceTooHigh, DO_DirUpValveImpedanceTooLow, DO_DirDownValveImpedanceTooHigh, DO_DirDownValveImpedanceTooLow;
#define Old_LimitSwitch1 *p_u8
#define Old_LimitSwitch2 *(p_u8 + 1)
#define Att *(p_u8 + 2)
#define AttenActive *(p_u8 + 3)
#define Ph *(p_u8 + 4)
#define CounterDir1 *(p_u8 + 5)
#define CounterDir2 *(p_u8 + 6)
#define Init *(p_u8 + 7)

#define Old_Control *p_int
#define MotionCntrl_out *(p_int + 1)
#define Smooth_out *(p_int + 2)
#define MC_out *(p_int + 3)
#define tmp *(p_int + 4)
#define MR *(p_int + 5)
#define E0 *(p_int + 6)
#define ME *(p_int + 7)
#define E *(p_int + 8)

	//=================================================================
	//==init
	if (Init != 123)
	{
		Init = 123;
		//
		Old_LimitSwitch1 = 0;
		Old_LimitSwitch2 = 0;
		Att = 0;
		AttenActive = 0;
		Ph = 0;
		CounterDir1 = 0;
		CounterDir2 = 0;
		//
		Old_Control = 0;
		MotionCntrl_out = 0;
		Smooth_out = 0;
		MC_out = 0;
		tmp = 0;
		MR = 0;
		E0 = 0;
		ME = 0;
		E = 0;
	}
	//======================================================Smooth(FB)=====================================================
	//==limit FeedBack
	if (FeedBack > 63000)
		FeedBack = FeedBack;
	// EndAttenuation(FB)
	if (Enable == 0)
		Control = 0;
	else
	{
		if ((Old_LimitSwitch1 == 0 && DI_LimitSwitchUp == 1) || (Old_LimitSwitch2 == 0 && DI_LimitSwitchDown == 1))
		{
			Old_Control = Control;
			Att = 1;
		}
		if (Att == 1 && ((Control > 0 && Old_Control < 0) || (Control < 0 && Old_Control > 0) || (DI_LimitSwitchUp == 0 && DI_LimitSwitchDown == 0)))
		{
			Att = 0;
			Old_Control = 0;
		}
		if (Att == 1)
		{
			if (Control < 0)
			{
				Control = -Control;
				if (Control > (327.67 * (float)PAR_EndattenuationValue))
				{
					Control = 327.67 * (float)PAR_EndattenuationValue;
				}
				Control = -Control;
			}
			else if (Control > (327.67 * (float)PAR_EndattenuationValue))
			{
				Control = 327.67 * (float)PAR_EndattenuationValue;
			}
		}

		AttenActive = Att;
		Old_LimitSwitch1 = DI_LimitSwitchUp;
		Old_LimitSwitch2 = DI_LimitSwitchDown;
	}
	//
	if (Control != 0 || MotionCntrl_out != 0)
		DO_SupplyVoltage = 1;
	else
		DO_SupplyVoltage = 0;
	if (Control != 0)
		CtrlAct = 1;
	else
		CtrlAct = 0;
	// Smooth(FB)
	if (PAR_DirDownAccel == 0)
		Ar = 65535;
	else
		Ar = 0x1E000 / 10 / PAR_DirDownAccel;
	if (PAR_DirUpAccel == 0)
		Af = 65535;
	else
		Af = 0x1E000 / 10 / PAR_DirUpAccel;
	if (PAR_DirDownDecel == 0)
		Dr = 4096;
	else
		Dr = 0x1E000 / 10 / PAR_DirUpDecel;
	if (PAR_DirDownDecel == 0)
		Df = 4096;
	else
		Df = 0x1E000 / 10 / PAR_DirUpDecel;
	//
	E = Control / 2 - Smooth_out;
	if (Smooth_out > 0 || (Smooth_out == 0 && Control > 0))
	{
		if (E < 0)
		{
			if (E <= (-4096))
				Smooth_out = -Df + Smooth_out;
			else
				Smooth_out = Smooth_out + (E * Df - 4095) / 4096;
			if (Smooth_out < 0)
				Smooth_out = 0;
		}
		else
		{
			if (E >= Af)
				Smooth_out = Smooth_out + Af;
			else
				Smooth_out = Smooth_out + E;
		}
	}
	else
	{
		if (Smooth_out < 0 || (Smooth_out == 0 && Control < 0))
		{
			if (E > 0)
			{
				if (E >= 4096)
					Smooth_out = Dr + Smooth_out;
				else
					Smooth_out = Smooth_out + (E * Dr + 4095) / 4096;
				if (Smooth_out > 0)
					Smooth_out = 0;
			}
			else
			{
				if (-E >= Ar)
					Smooth_out = -Ar + Smooth_out;
				else
					Smooth_out = Smooth_out + E;
			}
		}
	}
	//======================================================MotionControl(FB)=====================================================
	Zr = I_Full * PAR_DirDownMinCurrent / I_MAX;
	Kr = I_Full * PAR_DirDownMaxCurrent / I_MAX;
	Zf = I_Full * PAR_DirUpMinCurrent / I_MAX;
	Kf = I_Full * PAR_DirUpMaxCurrent / I_MAX;
	Kf = Kf - Zf;
	Kr = Kr - Zr;
	if (Smooth_out != 0)
	{
		if ((Ph & 3) != 0)
		{
			nnn = 1; //????????????????????????????????????????
			MR = Full * Smooth_out / JoyFull;
			Ph = Ph - 1;
			if (Ph == 0)
			{
				if (Smooth_out >= 0)
					MR = (Smooth_out * Kf / JoyFull + Zf) * (Full * 32768 / I_Full) / 65536;
				else
					MR = (Smooth_out * Kr / JoyFull + Zr) * (Full * 32768 / I_Full) / 65536;
				ME = 0;
			}
		}
		else
		{
			if ((Ph & 4) == 0)
			{
				nnn = 2; //????????????????????????????????????????
				Ph = Ph | 4;
				if (Smooth_out >= 0)
					MR = Rr * (65536 * I_Nom / I_Full) / 65536 * Zf / 65536;
				else
					MR = -Rr * (65536 * I_Nom / I_Full) / 65536 * Zr / 65536;
			}
			E0 = ME;
			if (Smooth_out >= 0)
				ME = (Smooth_out * Kf / JoyFull + Zf - FeedBack) * (Full * 32768 / I_Full) / 65536;
			else
				ME = (Smooth_out * Kr / JoyFull - Zr + FeedBack) * (Full * 32768 / I_Full) / 65536;
			tmp = MR + ME / 4 + ME - E0;
			//	printf("Smooth_out:\%4d\r\n\r\n",Smooth_out) ;
			if (Smooth_out >= 0)
			{
				if (tmp > 32767)
					MR = 32767;
				else if (tmp < 0)
					MR = 0;
				else
					MR = tmp;
			}
			else
			{
				if (tmp < -32767)
					MR = -32767;
				else if (tmp >= 0)
					MR = 0;
				else
					MR = tmp;
			}
		}
	}
	else
	{
		if ((Ph & 4) != 0)
		{
			Ph = Ph & 251;
			ME = 0;
			MR = 0;
		}
	}
	MC_out = MR;
	// CheckFault(FB)
	if (MC_out != 0)
	{
		if (CounterDir1 < 50)
			CounterDir1 = CounterDir1 + 1;
		else
		{
			if (FeedBack < 2000)
			{
				if (MC_out > 0)
					FaultActiveDir1 = 1;
				else
					FaultActiveDir2 = 1;
			}
			else
			{
				if (FeedBack > 2000)
				{
					FaultActiveDir1 = 0;
					FaultActiveDir2 = 0;
				}
			}
		}
	}
	else
	{
		CounterDir1 = 0;
		CounterDir2 = 0;
		if (MC_out == 0)
		{
			FaultActiveDir1 = 0;
			FaultActiveDir2 = 0;
		}
	}
	//
	*PWM_Up = 0;
	*PWM_Down = 0;
	if (MC_out < 0)
		*PWM_Down = -MC_out;
	if (MC_out > 0)
		*PWM_Up = MC_out;
	/*if(FaultActiveDir1||FaultActiveDir2)
	{
		*PWM_Down=0;
		*PWM_Up=0;
		DO_SupplyVoltage=0;
	}*/
	// ControlMotionErrors(FB)
}